#! /usr/bin/env python  
# coding=utf-8

import rospy
import numpy as np
import copy
from ti5arm.msg import ti5_polyJC_Lists, ti5_key_cmd
from ti5GrabFruits import sub_motorStat

class LISTEN_KEY_CMD:
    
    def __init__(self,use_time,cmd_rate):

        self.use_time = use_time
        self.cmd_rate = cmd_rate

        self.cJdeg = 0
        self.targJ = np.zeros((1,6), dtype=np.float)
        self.sub = rospy.Subscriber("ti5_key_cmd", ti5_key_cmd, self.ti5keycmd_cbf, queue_size=10)
        self.pub = rospy.Publisher("interPolyPoint_topic",ti5_polyJC_Lists,queue_size=1000)

        pass

    def ti5keycmd_cbf(self, key_cmd_msg):
        
        self.targJ[0,0] = self.cJdeg[0,0] + key_cmd_msg.joint1
        self.targJ[0,1] = self.cJdeg[0,1] + key_cmd_msg.joint2
        self.targJ[0,2] = self.cJdeg[0,2] + key_cmd_msg.joint3
        self.targJ[0,3] = self.cJdeg[0,3] + key_cmd_msg.joint4
        self.targJ[0,4] = self.cJdeg[0,4] + key_cmd_msg.joint5
        self.targJ[0,5] = self.cJdeg[0,5] + key_cmd_msg.joint6

        jointTarget = ti5_polyJC_Lists()
        # **************************config ctrl mode********************************
        jointTarget.numOfCmd = 6
        jointTarget.interpolyReq = 0 # external J space linear interpoly
        jointTarget.mode = 0
        jointTarget.header.stamp = rospy.Time().now()
        jointTarget.cmdRate = int(self.cmd_rate)
        # **************************************************************************
        
        poly_step = int(self.use_time*self.cmd_rate)
        #jointxRadList is deg

        cnJ = np.array(self.targJ-self.cJdeg,np.float)
        stepValue = cnJ/poly_step
        for i in range(1,poly_step+1):
            jointTarget.joint1RadList.append(self.cJdeg[0,0] + (i*stepValue[0,0]))
            jointTarget.joint2RadList.append(self.cJdeg[0,1] + (i*stepValue[0,1]))
            jointTarget.joint3RadList.append(self.cJdeg[0,2] + (i*stepValue[0,2]))
            jointTarget.joint4RadList.append(self.cJdeg[0,3] + (i*stepValue[0,3]))
            jointTarget.joint5RadList.append(self.cJdeg[0,4] + (i*stepValue[0,4]))
            jointTarget.joint6RadList.append(self.cJdeg[0,5] + (i*stepValue[0,5]))

        self.pub.publish(jointTarget)
        self.cJdeg = self.targJ
        
        print("goto: %.3f, %.3f, %.3f, %.3f, %.3f, %.3f"%(self.targJ[0,0],
                                                          self.targJ[0,1],
                                                          self.targJ[0,2],
                                                          self.targJ[0,3],
                                                          self.targJ[0,4],
                                                          self.targJ[0,5]) )
        pass

    def listen_key(self):
        
        print("开启键盘控制模式...")
        rate = rospy.Rate(1)
        rate.sleep()#waiter for system

        for i in range(0,10):
            self.cJdeg = sub_motorStat() # (1,6)
            if  type(self.cJdeg) != type(-1):
                break
            pass
        # self.cJdeg = np.zeros((1,6), dtype = np.float)
        if type(self.cJdeg) == type(-1):
            print("get arm sta falied please retry!")
            return 

        print("current joint deg: ", self.cJdeg)
        self.targJ = self.cJdeg
        rospy.spin()

        pass



    pass







if  __name__== "__main__":
    rospy.init_node("ti5arm_key_cmd_py_node")
  
    
    lkc = LISTEN_KEY_CMD(use_time=0.03, cmd_rate = 250)

    lkc.listen_key()

    pass


